#include <mc_common/msg_convert.h>

namespace msg_convert{

    // tf2::Quaternion -> Euler
    std::tuple<double, double, double> Quaternion2Euler(tf2::Quaternion quaternion){
        tf2::Matrix3x3 mat(quaternion);
        double roll, pitch, yaw;
        mat.getRPY(roll, pitch, yaw);
        return std::make_tuple(roll, pitch, yaw);
    }

    // geometry_msgs::Quaternion -> Euler
    std::tuple<double, double, double> GeometryQuaternion2Euler(geometry_msgs::Quaternion orientation){
        tf2::Quaternion quaternion;
        tf2::fromMsg(orientation, quaternion);
        return Quaternion2Euler(quaternion);
    }

    // Euler -> tf2::Quaternion
    tf2::Quaternion Euler2Quaternion(double roll, double pitch, double yaw){
        tf2::Quaternion quaternion;
        quaternion.setRPY(roll, pitch, yaw);
        return quaternion;
    }

    // Euler -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion Euler2GeometryQuaternion(double roll, double pitch, double yaw){
        geometry_msgs::Quaternion orientation;
        tf2::Quaternion quaternion = Euler2Quaternion(roll, pitch, yaw);
        orientation = tf2::toMsg(quaternion);
        return orientation;
    }


    // tf2::Transform -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Transform2Geometry(tf2::Transform transform_tf){
        geometry_msgs::TransformStamped transform_stamped;
        transform_stamped.transform = tf2::toMsg(transform_tf);
        return transform_stamped;
    }

    // geometry_msgs::TransformStamped -> tf2::Transform
    tf2::Transform Geometry2Transform(geometry_msgs::TransformStamped transform_stamped){
        tf2::Transform transform_tf;
        tf2::fromMsg(transform_stamped.transform, transform_tf);
        return transform_tf;  
    }

    // geometry_msgs::TransformStamped -> Eigen::Affine3f
    Eigen::Affine3f Geometry2Affine(geometry_msgs::TransformStamped transform){
        Eigen::Translation3f translation(transform.transform.translation.x,
            transform.transform.translation.y,
            transform.transform.translation.z);
        Eigen::Quaternionf rotation(transform.transform.rotation.w,
            transform.transform.rotation.x,
            transform.transform.rotation.y,
            transform.transform.rotation.z);     
        Eigen::Affine3f transform_matrix = translation * rotation;
        return transform_matrix;    
    }

    /***********************************************************
     * ToVec2f
     ***********************************************************/
    // Pose2D -> Vec2f
    Vec2f ToVec2f(Pose2D pose_2d){
        return Vec2f(pose_2d[0], pose_2d[1]);
    }

    /***********************************************************
     * ToPose2D
     ***********************************************************/
    // Pose2D -> string
    string Pose2D2Str(Pose2D pose){
        return "Pose2D(x: " + to_string(pose[0]) 
            + ", y: " + to_string(pose[1])
            + ", yaw: " + to_string(pose[2])
            + ")";
    }

    // Vec2f -> Pose2D
    Pose2D ToPose2D(Vec2f vec_2f){
        return Pose2D(vec_2f.x, vec_2f.y, 0.0f);
    }

    // pcl::PointXYZ -> Pose2D
    Pose2D ToPose2D(pcl::PointXYZ point){
        return Pose2D(point.x, point.y, 0.0f);
    }

    // geometry_msgs::Pose -> Pose2D
    Pose2D ToPose2D(geometry_msgs::Pose pose){
        return Pose2D(pose.position.x, pose.position.y, tf2::getYaw(pose.orientation));
    }

    /***********************************************************
     * ToGeometryPose
     ***********************************************************/
    // Pose2D -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(Pose2D pose){
        geometry_msgs::Pose pose_msg;
        pose_msg.position.x = pose[0];
        pose_msg.position.y = pose[1];
        pose_msg.orientation = Euler2GeometryQuaternion(0.0, 0.0, pose[2]);
        return pose_msg;
    }

    // Vec2f -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(Vec2f pose){
        geometry_msgs::Pose pose_msg;
        pose_msg.position.x = pose.x;
        pose_msg.position.y = pose.y;
        // pose_msg.orientation = Euler2GeometryQuaternion(0.0, 0.0, pose[2]);
        return pose_msg;
    }

    // geometry_msgs::Point -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(geometry_msgs::Point point){
        geometry_msgs::Pose pose_msg;
        pose_msg.position.x = point.x;
        pose_msg.position.y = point.y;
        pose_msg.position.z = point.z;
        return pose_msg;
    }

    // Pose2D -> geometry_msgs::PoseStamped
    geometry_msgs::PoseStamped ToGeometryPoseStamp(Pose2D pose, string frame_id){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header.frame_id = frame_id;
        pose_stamp.header.stamp = ros::Time();
        pose_stamp.pose = ToGeometryPose(pose);
        return pose_stamp;
    }

    // Vec2f -> geometry_msgs::PoseStamped
    geometry_msgs::PoseStamped ToGeometryPoseStamp(Vec2f pose, string frame_id){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header.frame_id = frame_id;
        pose_stamp.header.stamp = ros::Time();
        pose_stamp.pose = ToGeometryPose(pose);
        return pose_stamp;
    }

    // geometry_msgs::Point -> geometry_msgs::PoseStamped
    geometry_msgs::PoseStamped ToGeometryPoseStamp(geometry_msgs::Point point, string frame_id){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header.frame_id = frame_id;
        pose_stamp.header.stamp = ros::Time();
        pose_stamp.pose = ToGeometryPose(point);
        return pose_stamp;
    }

    /***********************************************************
     * ToPath
     ***********************************************************/
    // vector<Pose2D> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<Pose2D> poses, string frame_id){
        nav_msgs::Path path;
        path.header.frame_id = frame_id;
        path.header.stamp = ros::Time();
        for(size_t i=0; i<poses.size(); ++i){
            geometry_msgs::PoseStamped pose_stamp = ToGeometryPoseStamp(poses[i], frame_id);
            path.poses.emplace_back(pose_stamp);
        }
        return path;
    }

    // vector<Vec2f> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<Vec2f> poses, string frame_id){
        nav_msgs::Path path;
        path.header.frame_id = frame_id;
        path.header.stamp = ros::Time();
        for(size_t i=0; i<poses.size(); ++i){
            geometry_msgs::PoseStamped pose_stamp = ToGeometryPoseStamp(poses[i], frame_id);
            path.poses.emplace_back(pose_stamp);
        }
        return path;
    }

    // vector<geometry_msgs::Point> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<geometry_msgs::Point> points, string frame_id){
        nav_msgs::Path path;
        path.header.frame_id = frame_id;
        path.header.stamp = ros::Time();    
        for(size_t i=0; i<points.size(); ++i){
            geometry_msgs::PoseStamped pose_stamp = ToGeometryPoseStamp(points[i], frame_id);
            path.poses.emplace_back(pose_stamp);
        }
        return path;
    }


    /***********************************************************
     * ToPointCloud2
     ***********************************************************/
    sensor_msgs::PointCloud2 ToPointCloud2(vector<geometry_msgs::Point> points, string frame_id){
        sensor_msgs::PointCloud2 cloud_msg;
        cloud_msg.header.frame_id = frame_id;
        cloud_msg.header.stamp = ros::Time::now();
        cloud_msg.height = 1;
        cloud_msg.width = points.size();
        cloud_msg.is_dense = false;

        sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        modifier.resize(points.size());

        // 使用迭代器填充点云消息的数据
        sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
        for(const auto& point : points) {
            *iter_x = point.x;
            *iter_y = point.y;
            *iter_z = point.z;

            ++iter_x;
            ++iter_y;
            ++iter_z;
        }    
        return cloud_msg;    
    }

    /***********************************************************
     * Velocity2D <-> geometry_msgs::Twist
     ***********************************************************/
    // Velocity2D -> string
    string Velocity2D2Str(Velocity2D vel){
        return "Velocity2D(vx: " + to_string(vel[0]) 
            + ", vy: " + to_string(vel[1])
            + ", vth: " + to_string(vel[2])
            + ")";
    }

    // Velocity2D -> geometry_msgs::Twist
    geometry_msgs::Twist ToGeometryTwist(Velocity2D velocity){
        geometry_msgs::Twist twist;
        twist.linear.x = velocity[0];
        twist.linear.y = velocity[1];
        twist.angular.z = velocity[2];
        return twist;
    }

    // geometry_msgs::Twist -> Velocity2D
    Velocity2D ToVelocity2D(geometry_msgs::Twist twist){
        return Velocity2D(twist.linear.x, twist.linear.y, twist.angular.z);
    }


    // 转换2D数据为点云数据 sensor_data::Scan2DPtr -> sensor_data::PointCloudXYZPtr
    sensor_data::PointCloudXYZPtr convertToPointCloudXYZ(
        sensor_data::Scan2DPtr scan_ptr){
        sensor_data::PointCloudXYZPtr pointcloud_ptr(new sensor_data::PointCloudXYZ);
        for(size_t i=0; i<scan_ptr->ranges.size(); ++i){
            float angle = scan_ptr->angle_min + i * scan_ptr->angle_increment;
            float range = scan_ptr->ranges[i];
            pcl::PointXYZ point;
            point.x = range * cos(angle);
            point.y = range * sin(angle);
            point.z = 0.0;
            pointcloud_ptr->points.emplace_back(point);
        }
        pointcloud_ptr->header.frame_id = scan_ptr->header.frame_id; // 设置坐标系
        // pointcloud_ptr->header.stamp = scan_ptr->header.stamp.toNSec();
        pointcloud_ptr->height = 1; // 单层点云
        pointcloud_ptr->width = pointcloud_ptr->points.size();  
        pointcloud_ptr->is_dense = false; 
        return pointcloud_ptr;
    }


    // 转换点云数据  sensor_data::PointCloud2Ptr -> sensor_data::PointCloudXYZPtr
    sensor_data::PointCloudXYZPtr convertToPointCloudXYZ(
        sensor_data::PointCloud2Ptr pointcloud2_ptr){
        sensor_data::PointCloudXYZPtr pcl_cloud_ptr(new sensor_data::PointCloudXYZ);
        pcl::fromROSMsg(*pointcloud2_ptr, *pcl_cloud_ptr);
        return pcl_cloud_ptr;
    }

    // 转换到PointCloud2  sensor_data::PointCloudXYZPtr -> sensor_msgs::PointCloud2
    sensor_msgs::PointCloud2Ptr convertToPointCloud2(sensor_data::PointCloudXYZPtr pointcloud_ptr){
        sensor_data::PointCloud2Ptr output_ptr = boost::make_shared<sensor_data::PointCloud2>();
        if(!pointcloud_ptr) return nullptr;
        pcl::toROSMsg(*pointcloud_ptr, *output_ptr);
        output_ptr->header.frame_id = pointcloud_ptr->header.frame_id;
        output_ptr->header.stamp = ros::Time(pointcloud_ptr->header.stamp);    
        return output_ptr;        
    }

    // TF变换 pcl::PointCloud<pcl::PointXYZ>::Ptr  Eigen::Affine3f
    void transformPointXYZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, const Eigen::Affine3f &transform){
        // 使用 PCL 内置的 transformPointCloud 函数进行变换
        pcl::transformPointCloud(*cloud_in, *cloud_out, transform);
    }    

    // sensor_msgs::PointCloud2::Ptr -> sensor_msgs::PointCloud
    sensor_msgs::PointCloud convertToPointCloud(sensor_msgs::PointCloud2Ptr pointcloud2_ptr){
        sensor_msgs::PointCloud out_pointcloud;
        sensor_msgs::convertPointCloud2ToPointCloud(*pointcloud2_ptr, out_pointcloud);
        return out_pointcloud;
    }

} //namespace MapConvert